{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 第三次课程作业——基础矩阵、单应矩阵估计 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 任务  \n",
    "\n",
    "- 根据归一化八点法求取基本矩阵\n",
    "- 根据四点法求取单应矩阵\n",
    "- 以 images 文件夹里的 **SDC_0480.JPG** 和 **DSC_0481.JPG** 两幅图片为例\n",
    "\n",
    "<div align=center>\n",
    "<img src=\"images/DSC_0480.JPG\" width=200>\n",
    "<img src=\"images/DSC_0481.JPG\" width=200>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 示例代码"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **图像信息提取函数（SIFT特征点和EXIF信息）**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "class FeatureProcess:\n",
    "    \"\"\"\n",
    "    Simple SIFT feature process\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, image):\n",
    "        \"\"\"\n",
    "        Init\n",
    "        :param image: (np.ndarray): Image in RGB\n",
    "        \"\"\"\n",
    "        self.image = image\n",
    "        self.gray = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)\n",
    "        self.keypoints = None\n",
    "        self.descriptors = None\n",
    "\n",
    "    def extract_features(self):\n",
    "        \"\"\"\n",
    "        Extract SIFT features in image.\n",
    "        :return: (list, np.ndarray): keypoints, descriptors in image\n",
    "        \"\"\"\n",
    "        sift = cv2.SIFT_create()\n",
    "        keypoints, descriptors = sift.detectAndCompute(self.gray, None)\n",
    "\n",
    "        if len(keypoints) <= 20:\n",
    "            return None, None\n",
    "        else:\n",
    "            self.keypoints = keypoints\n",
    "            self.descriptors = descriptors\n",
    "            return keypoints, descriptors\n",
    "\n",
    "class PhotoExifInfo:\n",
    "    \"\"\"\n",
    "    Extract photo exif info\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, photo_path):\n",
    "        \"\"\"\n",
    "        init\n",
    "        :param photo_path: (str): photo path\n",
    "        \"\"\"\n",
    "        self.photo_path = photo_path\n",
    "        self.focal_length = None\n",
    "        self.image_width = None\n",
    "        self.image_length = None\n",
    "        self.sensor_pixel_size = None\n",
    "\n",
    "    def get_tags(self):\n",
    "        \"\"\"\n",
    "        Get tags with interested info\n",
    "        :return: None\n",
    "        \"\"\"\n",
    "        image_content = open(self.photo_path, 'rb')\n",
    "        tags = exifread.process_file(image_content)\n",
    "        self.focal_length = float(\n",
    "            tags['EXIF FocalLength'].values[0].num) / float(tags['EXIF FocalLength'].values[0].den)\n",
    "        self.image_width = float(tags['EXIF ExifImageWidth'].values[0])\n",
    "        self.image_length = float(tags['EXIF ExifImageLength'].values[0])\n",
    "        self.sensor_pixel_size = tags['MakerNote SensorPixelSize']\n",
    "\n",
    "    def get_intrinsic_matrix(self):\n",
    "        \"\"\"\n",
    "        Get intrinsic matrix of photo's camera\n",
    "        :return: (np.ndarray): intrinsic matrix K\n",
    "        \"\"\"\n",
    "        K = np.zeros([3, 3])\n",
    "        dx = self.sensor_pixel_size.values[0].num / self.sensor_pixel_size.values[0].den / self.image_width\n",
    "        dy = self.sensor_pixel_size.values[1].num / self.sensor_pixel_size.values[1].den / self.image_length\n",
    "        fu = self.focal_length / dx\n",
    "        fv = self.focal_length / dy\n",
    "        u0 = self.image_width / 2\n",
    "        v0 = self.image_length / 2\n",
    "        K[0][0] = fu\n",
    "        K[1][1] = fv\n",
    "        K[0][2] = u0\n",
    "        K[1][2] = v0\n",
    "        K[2][2] = 1\n",
    "        return K\n",
    "\n",
    "    def get_area(self):\n",
    "        \"\"\"\n",
    "        Get area of photo\n",
    "        :return: (int): area\n",
    "        \"\"\"\n",
    "        return int(self.image_width * self.image_length)\n",
    "\n",
    "    def get_diam(self):\n",
    "        \"\"\"\n",
    "        Get diam of photo\n",
    "        :return: (int): diam\n",
    "        \"\"\"\n",
    "        return int(max(self.image_width, self.image_length))\n",
    "\n",
    "def build_img_info(img_root):\n",
    "    \"\"\"\n",
    "    Get info(img,feat,K) from img\n",
    "    :param img_root: (str): images root\n",
    "    :return: (list[np.ndarray], list[dict], list[np.ndarray]): info from img\n",
    "    \"\"\"\n",
    "    imgs = []\n",
    "    feats = []\n",
    "    K = []\n",
    "    for i, name in enumerate(os.listdir(img_root)):\n",
    "        if '.jpg' in name or '.JPG' in name:\n",
    "            path = os.path.join(img_root, name)\n",
    "            img = cv2.imread(path)\n",
    "            imgs.append(img)\n",
    "            feature_process = FeatureProcess(img)\n",
    "            kpt, des = feature_process.extract_features()\n",
    "            photo_info = PhotoExifInfo(path)\n",
    "            photo_info.get_tags()\n",
    "            K.append(photo_info.get_intrinsic_matrix())\n",
    "            A = photo_info.get_area()\n",
    "            D = photo_info.get_diam()\n",
    "            feats.append({'kpt': kpt, 'des': des, 'A': A, 'D': D})\n",
    "    return imgs, feats, K"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **归一化八点法和四点法（基本矩阵和单应矩阵）**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_matches(des_query, des_train):\n",
    "    \"\"\"\n",
    "    Match features between query and train\n",
    "    :param des_query: (np.ndarray): query descriptors\n",
    "    :param des_train: (np.ndarray): train descriptors\n",
    "    :return: (list[cv2.DMatch]): Match info\n",
    "    \"\"\"\n",
    "    bf = cv2.BFMatcher(cv2.NORM_L2)\n",
    "    matches = bf.knnMatch(des_query, des_train, k=2)\n",
    "\n",
    "    good = []\n",
    "    for m, m_ in matches:\n",
    "        # Ratio is 0.6 ,which to remain enough features\n",
    "        if m.distance < 0.6 * m_.distance:\n",
    "            good.append(m)\n",
    "    return good\n",
    "\n",
    "def get_match_point(p, p_, matches):\n",
    "    \"\"\"\n",
    "    Find matched keypoints\n",
    "    :param p: (list[cv2.KeyPoint]): query keypoints\n",
    "    :param p_: (list[cv2.KeyPoint]): train keypoints\n",
    "    :param matches: (list[cv2.DMatch]): match info between query and train\n",
    "    :return: (np.ndarray, np.ndarray): matched keypoints between query and train\n",
    "    \"\"\"\n",
    "    points_query = np.asarray([p[m.queryIdx].pt for m in matches])\n",
    "    points_train = np.asarray([p_[m.trainIdx].pt for m in matches])\n",
    "    return points_query, points_train\n",
    "\n",
    "def normalize(pts, T=None):\n",
    "    \"\"\"\n",
    "    normalize points\n",
    "    :param pts: (np.ndarray): points to be normalized\n",
    "    :param T: (np.ndarray): IS None means we need to computer T\n",
    "    :return: (np.ndarray, np.ndarray): normalized points and T\n",
    "    \"\"\"\n",
    "    if T is None:\n",
    "        u = np.mean(pts, 0)\n",
    "        d = np.sum(np.sqrt(np.sum(np.power(pts, 2), 1)))\n",
    "        T = np.array([\n",
    "            [np.sqrt(2) / d, 0, -(np.sqrt(2) / d * u[0])],\n",
    "            [0, np.sqrt(2) / d, -(np.sqrt(2) / d * u[1])],\n",
    "            [0, 0, 1]\n",
    "        ])\n",
    "    return homoco_pts_2_euco_pts(np.matmul(T, euco_pts_2_homoco_pts(pts).T).T), T\n",
    "\n",
    "def homoco_pts_2_euco_pts(pts):\n",
    "    \"\"\"\n",
    "    Homogeneous coordinate to Euclidean coordinates\n",
    "    :param pts: (np.ndarray): Homogeneous coordinate\n",
    "    :return: (np.ndarray): Euclidean coordinates\n",
    "    \"\"\"\n",
    "    if len(pts.shape) == 1:\n",
    "        pts = pts.reshape(1, -1)\n",
    "    res = pts / pts[:, -1, None]\n",
    "    return res[:, :-1].squeeze()\n",
    "\n",
    "def euco_pts_2_homoco_pts(pts):\n",
    "    \"\"\"\n",
    "    Euclidean coordinate to Homogeneous coordinates\n",
    "    :param pts: (np.ndarray): Euclidean coordinate\n",
    "    :return: (np.ndarray): Homogeneous coordinates\n",
    "    \"\"\"\n",
    "    if len(pts.shape) == 1:\n",
    "        pts = pts.reshape(1, -1)\n",
    "    one = np.ones(pts.shape[0])\n",
    "    res = np.c_[pts, one]\n",
    "    return res.squeeze()\n",
    "\n",
    "def estimate_fundamental(pts1, pts2, num_sample=8):\n",
    "    n = pts1.shape[0]\n",
    "    pts_index = range(n)\n",
    "    sample_index = random.sample(pts_index, num_sample)\n",
    "    p1 = pts1[sample_index, :]\n",
    "    p2 = pts2[sample_index, :]\n",
    "    n = len(sample_index)\n",
    "    p1_norm, T1 = normalize(p1, None)\n",
    "    p2_norm, T2 = normalize(p2, None)\n",
    "    w = np.zeros((n, 9))\n",
    "    for i in range(n):\n",
    "        w[i, 0] = p1_norm[i, 0] * p2_norm[i, 0]\n",
    "        w[i, 1] = p1_norm[i, 1] * p2_norm[i, 0]\n",
    "        w[i, 2] = p2_norm[i, 0]\n",
    "        w[i, 3] = p1_norm[i, 0] * p2_norm[i, 1]\n",
    "        w[i, 4] = p1_norm[i, 1] * p2_norm[i, 1]\n",
    "        w[i, 5] = p2_norm[i, 1]\n",
    "        w[i, 6] = p1_norm[i, 0]\n",
    "        w[i, 7] = p1_norm[i, 1]\n",
    "        w[i, 8] = 1\n",
    "\n",
    "    U, sigma, VT = np.linalg.svd(w)\n",
    "    f = VT[-1, :].reshape(3, 3)\n",
    "    U, sigma, VT = np.linalg.svd(f)\n",
    "    sigma[2] = 0\n",
    "    f = U.dot(np.diag(sigma)).dot(VT)\n",
    "    f = T2.T.dot(f).dot(T1)\n",
    "    return f\n",
    "\n",
    "def estimate_homo(pts1, pts2, num_sample=4):\n",
    "    n = pts1.shape[0]\n",
    "    pts_index = range(n)\n",
    "    sample_index = random.sample(pts_index, num_sample)\n",
    "    p1 = pts1[sample_index, :]\n",
    "    p2 = pts2[sample_index, :]\n",
    "    n = len(sample_index)\n",
    "    w = np.zeros((n * 2, 9))\n",
    "    for i in range(n):\n",
    "        w[2 * i, 0] = p1[i, 0]\n",
    "        w[2 * i, 1] = p1[i, 1]\n",
    "        w[2 * i, 2] = 1\n",
    "        w[2 * i, 3] = 0\n",
    "        w[2 * i, 4] = 0\n",
    "        w[2 * i, 5] = 0\n",
    "        w[2 * i, 6] = -p1[i, 0] * p2[i, 0]\n",
    "        w[2 * i, 7] = -p1[i, 1] * p2[i, 0]\n",
    "        w[2 * i, 8] = -p2[i, 0]\n",
    "        w[2 * i + 1, 0] = 0\n",
    "        w[2 * i + 1, 1] = 0\n",
    "        w[2 * i + 1, 2] = 0\n",
    "        w[2 * i + 1, 3] = p1[i, 0]\n",
    "        w[2 * i + 1, 4] = p1[i, 1]\n",
    "        w[2 * i + 1, 5] = 1\n",
    "        w[2 * i + 1, 6] = -p1[i, 0] * p2[i, 1]\n",
    "        w[2 * i + 1, 7] = -p1[i, 1] * p2[i, 1]\n",
    "        w[2 * i + 1, 8] = -p2[i, 1]\n",
    "    U, sigma, VT = np.linalg.svd(w)\n",
    "    h = VT[-1, :].reshape(3, 3)\n",
    "    return h"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def build_F_H_pair_match(feats):\n",
    "    \"\"\"\n",
    "    Build F, H, pair and match\n",
    "    :param feats: (list[dict]): feat of imgs\n",
    "    :return: (np.ndarray, np.ndarray, dict, dict): F, H, pair of imgs, match of pairs\n",
    "    \"\"\"\n",
    "\n",
    "\n",
    "    pair = dict()\n",
    "    match = dict()\n",
    "\n",
    "    for i in range(len(feats)):\n",
    "        for j in range(i + 1, len(feats)):\n",
    "            print(i, j)\n",
    "            matches = get_matches(\n",
    "                feats[i]['des'], feats[j]['des'])\n",
    "            pts1, pts2 = get_match_point(\n",
    "                feats[i]['kpt'], feats[j]['kpt'], matches)\n",
    "            assert pts1.shape == pts2.shape\n",
    "            # Need 8 points to estimate models\n",
    "            if pts1.shape[0] < 8:\n",
    "                continue\n",
    "\n",
    "            F_single = estimate_fundamental(pts1, pts2)\n",
    "            H_single = estimate_homo(pts1, pts2)\n",
    "\n",
    "            if pts1.shape[0] < 8:\n",
    "                continue\n",
    "\n",
    "            pair.update({(i, j): {'pts1': pts1, 'pts2': pts2}})\n",
    "            match.update({(i, j): {'match': matches}})\n",
    "\n",
    "\n",
    "    return F_single, H_single, pair, match"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 结果测试"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **导入所需模块**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import random\n",
    "import os\n",
    "import exifread"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **读取图像、提取特征点并计算基础矩阵和单应矩阵**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0 1\n",
      "The Fundamental Matrix is:\n",
      " [[ 2.04145376e-10  2.36540296e-08 -1.73467054e-05]\n",
      " [-2.17166341e-08  6.62880899e-10 -8.21073367e-05]\n",
      " [ 1.74208105e-05  7.92261019e-05  4.78852145e-03]]\n",
      "The Homography Matrix is:\n",
      " [[ 1.13554423e-02 -9.44761813e-05 -1.53000794e-01]\n",
      " [-4.50809624e-04  1.12960107e-02  9.88028524e-01]\n",
      " [-5.55802425e-07  1.25720789e-07  1.15600657e-02]]\n"
     ]
    }
   ],
   "source": [
    "img_root = 'images/'\n",
    "imgs, feats, K = build_img_info(img_root)\n",
    "F, H, pair, match = build_F_H_pair_match(feats)\n",
    "print(\"The Fundamental Matrix is:\\n\", F)\n",
    "print(\"The Homography Matrix is:\\n\", H)"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "2fd6ff00ff4a419d324d5b7e4b1b0789b8ee895e93e15d812a34184c59464f6c"
  },
  "kernelspec": {
   "display_name": "Python 3.8.11 64-bit ('MyCV': conda)",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.11"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
